#include "motor.h"
#include "tim.h"
#include "main.h"
#include "usart.h"
#include "stdio.h"
#include "math.h"


TIM_HandleTypeDef* throttle;

//init the throttle of motors 
void Mot_init(TIM_HandleTypeDef* thr)
{
	throttle=thr;
	HAL_TIM_PWM_Start(throttle,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(throttle,TIM_CHANNEL_2);
}

void Mot_exec_left(int16_t pwm)
{
	__HAL_TIM_SetCompare(throttle,TIM_CHANNEL_2,pwm);
}
void Mot_exec_right(int16_t pwm)
{
	__HAL_TIM_SetCompare(throttle,TIM_CHANNEL_1,pwm);
}

void Mot_stop_left()//左轮停
{
	HAL_GPIO_WritePin(LF_A_GPIO_Port,LF_A_Pin,GPIO_PIN_RESET);
	HAL_GPIO_WritePin(LF_B_GPIO_Port,LF_B_Pin,GPIO_PIN_RESET);
}
void Mot_stop_right()//右轮停
{
	HAL_GPIO_WritePin(RT_A_GPIO_Port,RT_A_Pin,GPIO_PIN_RESET);
	HAL_GPIO_WritePin(RT_B_GPIO_Port,RT_B_Pin,GPIO_PIN_RESET);
}

void Set_Left_MotorVoltageAndDirection(int16_t Pwm)
{
	int16_t pwm;
	if(Pwm<0)
	{
		HAL_GPIO_WritePin(LF_A_GPIO_Port,LF_A_Pin,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(LF_B_GPIO_Port,LF_B_Pin,GPIO_PIN_SET);
	}
	else if(Pwm>0)
	{
		HAL_GPIO_WritePin(LF_A_GPIO_Port,LF_A_Pin,GPIO_PIN_SET);
		HAL_GPIO_WritePin(LF_B_GPIO_Port,LF_B_Pin,GPIO_PIN_RESET);
	}
	pwm = fabs(Pwm);
  Mot_exec_left(pwm);
}

void Set_Right_MotorVoltageAndDirection(int16_t Pwm)
{
	int16_t pwm;
	if(Pwm<0)
	{
		HAL_GPIO_WritePin(RT_A_GPIO_Port,RT_A_Pin,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(RT_B_GPIO_Port,RT_B_Pin,GPIO_PIN_SET);
	}
	else if(Pwm>0)
	{
		HAL_GPIO_WritePin(LF_A_GPIO_Port,LF_A_Pin,GPIO_PIN_SET);
		HAL_GPIO_WritePin(LF_B_GPIO_Port,LF_B_Pin,GPIO_PIN_RESET);
	}
	pwm = fabs(Pwm);
  Mot_exec_right(pwm);
}



